// 切换成位置控制
#include "rclcpp/rclcpp.hpp"
#include "dxl_test/xm430_driver.hpp"
#include <thread>

using namespace std::chrono_literals;

int main()
{
    int dxl_id_1 = 0;
    int dxl_id_2 = 20;
    int dxl_id_3 = 30;
    int dxl_id_4 = 40;
    int dxl_id_5 = 50;

    auto driver = XM430Driver("/dev/ttyUSB0");
    driver.init();
    std::this_thread::sleep_for(1s);

    // TEST Position control
    driver.activateWithPositionMode(dxl_id_1);
    driver.activateWithPositionMode(dxl_id_2);
    driver.activateWithPositionMode(dxl_id_3);
    driver.activateWithPositionMode(dxl_id_4);
    driver.activateWithPositionMode(dxl_id_5);
    driver.setTargetPositionRadian(dxl_id_1, 0);
    driver.setTargetPositionRadian(dxl_id_2, 0);
    driver.setTargetPositionRadian(dxl_id_3, 0);
    driver.setTargetPositionRadian(dxl_id_4, 0);
    driver.setTargetPositionRadian(dxl_id_5, 0);
    std::this_thread::sleep_for(3s);
    double position = driver.getPositionRadian(dxl_id_1);
    double position2 = driver.getPositionRadian(dxl_id_2);
    double position3 = driver.getPositionRadian(dxl_id_3);
    double position4 = driver.getPositionRadian(dxl_id_4);
    double position5 = driver.getPositionRadian(dxl_id_5);
    std::cout << "Position motor 1 : " << position << std::endl;
    std::cout << "Position motor 2 : " << position << std::endl;
    std::cout << "Position motor 3 : " << position << std::endl;
    std::cout << "Position motor 4 : " << position << std::endl;
    std::cout << "Position motor 5 : " << position << std::endl;
    driver.deactivate(dxl_id_1);
    driver.deactivate(dxl_id_2);
    driver.deactivate(dxl_id_3);
    driver.deactivate(dxl_id_4);
    driver.deactivate(dxl_id_5);

    return 0;
}
